import os
import sys

sys.path.append(".")

import math
import numpy as np
# import converter
import matplotlib
matplotlib.use('TkAgg')
from matplotlib import pyplot as plt
from matplotlib.lines import Line2D
from mpl_toolkits.mplot3d import Axes3D
from mpl_toolkits.mplot3d.art3d import Line3D
import time_processor as tp
import geo_processor as gp
from scipy import interpolate


def read_tum_data(tum_path):

    result = []

    num = 0
    with open(tum_path) as f:
        for line in f:
            if line[0] != '#':
                pass
            num = num + 1 
            items = line.split(' ')
    
            result1 = []
            result1.append(float(items[0])) # timestamp
            result1.append(float(items[1])) # data1
            result1.append(float(items[2])) # data2
            result1.append(float(items[3])) # data3
            result1.append(float(items[4])) # data4
            result1.append(float(items[5])) # data5
            result1.append(float(items[6])) # data6
            result1.append(float(items[7])) # data7

            result.append(result1)

    return np.array(result)

def read_image_timestamp(image_timestamp_path):

    result = []

    num = 0
    with open(image_timestamp_path) as f:
        for line in f:
            if line[0] != '#':
                pass
            num = num + 1 
            result.append(float(line)) # timestamp
    
    return result

def change_timestamp_and_show(tum_result, image_timestamp, save_path):
    output_tum = open(save_path, "w+")
    
    i = 0
    for index in range(len(image_timestamp)):
        print(abs(tum_result[i,0] - image_timestamp[index] - 18470000000))
        if abs(tum_result[i,0] - image_timestamp[index] - 18470000000) < 10000000:
            P_wc = np.array([tum_result[i,1], tum_result[i,2], tum_result[i,2]])
            R_wc = np.array()
            output_tum.write("{:.0f}".format(i))
            output_tum.write(" ")
            output_tum.write("{:.12f}".format(tum_result[i,1]))
            output_tum.write(" ")
            output_tum.write("{:.12f}".format(tum_result[i,2]))
            output_tum.write(" ")
            output_tum.write("{:.12f}".format(tum_result[i,3]))
            output_tum.write(" ")
            output_tum.write("{:.12f}".format(tum_result[i,4]))
            output_tum.write(" ")
            output_tum.write("{:.12f}".format(tum_result[i,5]))
            output_tum.write(" ")
            output_tum.write("{:.12f}".format(tum_result[i,6]))
            output_tum.write(" ")
            output_tum.write("{:.12f}".format(tum_result[i,7]))
            output_tum.write("\n")
            i = i + 1

    output_tum.close()
    show(tum_result)

def show(data):
    fig = plt.figure()
    ax = Axes3D(fig)
    ax.scatter(data[:,1], data[:,2], data[:,3])
    plt.show()

if __name__ == "__main__":
    path2 = r"/media/cjg/Elements/pano20210902/pro2/data/image_timestamp.txt"
    image_timestamp = read_image_timestamp(path2)

    path1 = r"/shared/HBKit/trajectory_all.tum"
    save_path = r"/shared/HBKit/trajectory_all_new.tum"
    R = []
    t = []

    tum_result = read_tum_data(path1)
    change_timestamp_and_show(tum_result, image_timestamp, R, t, save_path)